#include <iostream>
#include "hybridIntegration.h"
#define ERROR 0.05

HybridIntegration::HybridIntegration() : IntegrationMethod(){
}

HybridIntegration::HybridIntegration(double _deltaT, double _g, std::vector<State> _s0,
                                     std::vector< std::vector<Particle *> > &_p, std::vector< std::vector<Spring *> > &_s,
                                     bool _activeAir, int _airType, double _airPeriod, double _airRepeat, double _airViscosity, Vector3D _airDirection)
                                     : IntegrationMethod(_deltaT, _g, _s0, _p, _s, _activeAir, _airType, _airPeriod, _airRepeat, _airViscosity, _airDirection)
{
}

HybridIntegration::~HybridIntegration(){
}

void HybridIntegration::calculate(bool _firstIteration, unsigned int _fabricPosition, double _fraction){
    //guardando dados
    State s = currentState.at(_fabricPosition);
    double t = s.getT();
    unsigned int dof = s.getDOF();
    Matrix reference = s.getReferenceMatrix();
    std::vector< std::pair<bool, Object *> > collision = s.getCollision();

    currentDeltaT = deltaT / _fraction;
    //calculando


    std::vector<Particle *> particlesI = particles.at(_fabricPosition);
    Vector *resultanteForce = populateResultantForces(s, _fabricPosition, EBERHARDT2000);
    Matrix *mass = populateM(s,_fabricPosition);
    Matrix inverseMass = (*mass).diagonalInverse();
    Matrix k(dof, dof);
    Matrix c(dof, dof);
    repopulateK_C(s, _fabricPosition, &k, &c);

    Vector currentVelocity = s.getVelocity();
    Vector currentPosition = s.getPosition();
    Matrix coefficient = matrixCoefficient(k, c, inverseMass, dof);
    Vector independent = matrixIndependentTerms(k, inverseMass, *resultanteForce, currentVelocity, currentPosition);


    Vector velocity(dof);

    if(_firstIteration)
    {
        velocity = currentVelocity.scalarProduct(2.0);
    }
    else
    {
        velocity = (currentVelocity.scalarProduct(2.0)) - (previousState.at(_fabricPosition)).getVelocity();
    }

    Matrix q(dof, dof);

    for(unsigned int i=0; i<coefficient.getRow(); i++)
    {
        for(unsigned int j=0; j<coefficient.getColumn();j++)
        {
            if(i!=j)
            {
                q[i][j]=(coefficient[i][j]*(-1.0))/coefficient[i][i];
            }
        }

    }

    Vector g(dof);
    for(unsigned int i=0; i<independent.getRow();i++)
    {
        g[i] = independent[i]/coefficient[i][i];
    }


    bool converge = false;

    int iteration = 0;
    do
    {

        iteration++;

        Vector vel = velocity;
        velocity = (q * vel) + g;

        Vector r = (coefficient * velocity) - independent;
        double residuo = r.length();

         /*double max = -1;
         double max2 = -1;
         for(unsigned int i= 0; i < dof; i++){
             double aux = (velocity[i] - vel[i]);
             if (aux > max) max = aux;
             if (fabs(velocity[i]) > max2) max2 = fabs(velocity[i]);
         }

        double residuo = max /max2;*/
        //double residuo = (velocity - vel).length();

        if (residuo < ERROR)
        {
            converge = true;
        }

    }while(converge==false);

    s.setVelocity(velocity);

    s.setPosition(currentPosition + (velocity.scalarProduct(currentDeltaT)));

    //atualizando
    previousState.at(_fabricPosition) = currentState.at(_fabricPosition);
    currentState.at(_fabricPosition) =s;
    currentState.at(_fabricPosition).setT_DOF_Ref_Col(t + currentDeltaT, dof, reference, collision);
    delete mass;
    delete resultanteForce;
}


void HybridIntegration::repopulateK_C(const State &_state, unsigned int _i, Matrix *_k, Matrix *_c){
    Matrix k_g;
    Matrix c_g;
    const Matrix &ref = _state.getReferenceMatrix();
    //bool delta;

    std::vector<Spring *> springsI = springs.at(_i);
    unsigned int size = springsI.size();


    for(unsigned int i=0; i<size; i++)
    {

        Spring *s = springsI.at(i);

        if (s->getType() != STRUCTURAL_HORIZONTAL && s->getType() != STRUCTURAL_VERTICAL) continue;

        //delta = s->positiveDelta();

        k_g = s->getK_();
        c_g = s->getC_();

        for(unsigned int j = 0; j < 3; j++){
            //pegando a massa de inicial
            if(ref[s->getParticleBegin()][j] != -1){
                for(unsigned int k = 0; k < 3; k++){
                    //comparando com ela mesma
                    if(ref[s->getParticleBegin()][k] != -1){
//                        if (delta)
                        (*_k)[    (int)ref[s->getParticleBegin()][j]    ][     (int)ref[s->getParticleBegin()][k]    ] += k_g[j][k];
                        (*_c)[    (int)ref[s->getParticleBegin()][j]    ][     (int)ref[s->getParticleBegin()][k]    ] += c_g[j][k];
                    }
                    //comparando com a massa final
                    if(ref[s->getParticleEnd()][k] != -1){
//                        if (delta)
                        (*_k)[    (int)ref[s->getParticleBegin()][j]   ][    (int)ref[s->getParticleEnd()][k]   ] += k_g[j][k+3];
                        (*_c)[    (int)ref[s->getParticleBegin()][j]   ][    (int)ref[s->getParticleEnd()][k]   ] += c_g[j][k+3];
                    }
                }
            }
            //pegando a massa final
            if(ref[s->getParticleEnd()][j] != -1){
                for(unsigned int k = 0; k < 3; k++){
                    //comparando com a massa inicial
                    if(ref[s->getParticleBegin()][k] != -1){
//                        if (delta)
                        (*_k)[    (int)ref[s->getParticleEnd()][j]  ][    (int)ref[s->getParticleBegin()][k]    ] += k_g[j+3][k];
                        (*_c)[    (int)ref[s->getParticleEnd()][j]  ][    (int)ref[s->getParticleBegin()][k]    ] += c_g[j+3][k];
                    }
                    //comparando com ela mesma
                    if(ref[s->getParticleEnd()][k] != -1){
//                        if (delta)
                        (*_k)[    (int)ref[s->getParticleEnd()][j] ][    (int)ref[s->getParticleEnd()][k]   ] += k_g[j+3][k+3];
                        (*_c)[    (int)ref[s->getParticleEnd()][j] ][    (int)ref[s->getParticleEnd()][k]   ] += c_g[j+3][k+3];
                    }
                }
            }
        }
    }

}
Matrix HybridIntegration::matrixCoefficient(const Matrix &_k, const Matrix &_c, const Matrix &_m, unsigned int _dof)
{
    Matrix result(_dof, _dof);
    Matrix identity(_dof, _dof, IDENTITY_MATRIX);

    Matrix rigidity = (_m *_k)*(currentDeltaT*currentDeltaT);
    Matrix damp = (_m *_c)*(currentDeltaT);
    result = (identity-rigidity)-damp;
    return result;
}

Vector HybridIntegration::matrixIndependentTerms(const Matrix &_k, const Matrix &_m, Vector _resultantForce, const Vector &_currentVelocity, const Vector &_currentPosition)
{
    Vector forcePerMass = (_m * _resultantForce);
    Vector f = (forcePerMass).scalarProduct(currentDeltaT);
    Vector rigidity = ((_m * _k) * _currentPosition);
    Vector r = rigidity.scalarProduct(currentDeltaT);
    Vector result = (_currentVelocity + f) + r;
    return result;
}
